package FRC2856.Robot;

import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.SpeedController;

public class Shooter{
    
    public boolean maxPower;
    public double leftOutput,rightOutput;
    private double Kp,Ki,Kd;
    public Encoder leftEnc,rightEnc;
    public PIDController leftPID,rightPID;
    public SpeedController leftMotor,rightMotor;
    
    public Shooter(){
        maxPower = false;
        Kp = 0.03;
        Ki = 0.003;
        Kd = 0.0;
        leftEnc = new Encoder(RobotMap.shtrLeftEncoderModule,RobotMap.shtrLeftEncoderChannelA,RobotMap.shtrLeftEncoderModule,RobotMap.shtrLeftEncoderChannelB,RobotMap.shtrLeftEncoderDirection,EncodingType.k1X);
        leftEnc.setDistancePerPulse(0.005818);
        leftEnc.start();
        rightEnc = new Encoder(RobotMap.shtrRightEncoderModule,RobotMap.shtrRightEncoderChannelA,RobotMap.shtrRightEncoderModule,RobotMap.shtrRightEncoderChannelB,RobotMap.shtrRightEncoderDirection,EncodingType.k1X);
        rightEnc.setDistancePerPulse(0.005818);
        rightEnc.start();
        leftMotor = new Jaguar(RobotMap.shtrLeftMotorModule, RobotMap.shtrLeftMotorChannel);
        rightMotor = new Jaguar(RobotMap.shtrRightMotorModule, RobotMap.shtrRightMotorChannel);
        
        leftEnc.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
        rightEnc.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
//        leftPID = new PIDController(Kp, Ki, Kd, leftEnc, leftMotor, PIDController.kDefaultPeriod);
//        rightPID = new PIDController(Kp, Ki, Kd, rightEnc, rightMotor, PIDController.kDefaultPeriod);
        leftPID = new PIDController(Kp, Ki, Kd, leftEnc, leftMotor, 0.01);    // 10 ms control loop
        rightPID = new PIDController(Kp, Ki, Kd, rightEnc, rightMotor, 0.01); // 10 ms control loop
        leftPID.setContinuous();
        rightPID.setContinuous();
        leftPID.setOutputRange(0.0, 0.75);
        rightPID.setOutputRange(0.0, 0.75);
    }

    public double[] decrementP(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        p -= 0.01;
        leftPID.setPID(p, i, d);
        returnValue[0] = p;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        p -= 0.01;
        rightPID.setPID(p, i, d);
        returnValue[1] = p;
        return returnValue;
    }

    public double[] decrementI(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        i -= 0.001;
        leftPID.setPID(p, i, d);
        returnValue[0] = i;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        i -= 0.001;
        rightPID.setPID(p, i, d);
        returnValue[1] = i;
        return returnValue;
    }

    public double[] decrementD(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        d -= 0.01;
        leftPID.setPID(p, i, d);
        returnValue[0] = d;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        d -= 0.01;
        rightPID.setPID(p, i, d);
        returnValue[1] = d;
        return returnValue;
    }

    public void disable(){
        spinStop();
    }

    public double[] getEncoderDist(){
        double[] returnValue = new double[2];
        returnValue[0] = leftEnc.getDistance();
        returnValue[1] = rightEnc.getDistance();
        return returnValue;
    }

    public double[] getEncoderRate(){
        double[] returnValue = new double[2];
        returnValue[0] = leftEnc.getRate();
        returnValue[1] = rightEnc.getRate();
        return returnValue;
    }

    public double[] getPID(){
        double[] returnValue = new double[2];
        returnValue[0] = leftPID.get();
        returnValue[1] = rightPID.get();
        return returnValue;
    }

    public double[] incrementP(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        p += 0.01;
        leftPID.setPID(p, i, d);
        returnValue[0] = p;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        p += 0.01;
        rightPID.setPID(p, i, d);
        returnValue[1] = p;
        return returnValue;
    }

   public double[] incrementI(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        i += 0.001;
        leftPID.setPID(p, i, d);
        returnValue[0] = i;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        i += 0.001;
        rightPID.setPID(p, i, d);
        returnValue[1] = i;
        return returnValue;
    }

   public double[] incrementD(){
        double p,i,d;
        double[] returnValue = new double[2];
        p = leftPID.getP();
        i = leftPID.getI();
        d = leftPID.getD();
        d += 0.01;
        leftPID.setPID(p, i, d);
        returnValue[0] = d;
        p = rightPID.getP();
        i = rightPID.getI();
        d = rightPID.getD();
        d += 0.01;
        rightPID.setPID(p, i, d);
        returnValue[1] = d;
        return returnValue;
    }

    public void maxPower(boolean enable){
        if(enable){
            // Turn off shooter to conserve power
            if(leftPID.isEnable() || rightPID.isEnable()){
                // Turn off PID
            }
            else{
                // Store effort and set to zero
            }
            //maxPower = true; // Uncomment once maxPower is fully implemented
        }
        else{
            // Return shooter to previous state
            if(false){ // need to check whether PID was enabled
                // Turn on PID (ramping?)
            }
            else{
                // Restore previous effort (ramping?)
            }
            maxPower = false;
        }
    }

    public boolean pidStatus(){
        return (leftPID.isEnable() || rightPID.isEnable());
    }

    public void setOutputRange(double range){
        leftPID.setOutputRange(0.0, range);
        rightPID.setOutputRange(0.0, range);
    }

    public void spinEffort(double effort){
        spinEffort(effort, effort);
    }

    public void spinEffort(double left, double right){
        if( !(leftPID.isEnable() || rightPID.isEnable()) ){
            if(maxPower){
                // Store new effort values
            }
            else{
                leftMotor.set(left);
                rightMotor.set(right);
            }
        }
    }

    public void spinRate(double left, double right){
        if(!maxPower){
            leftPID.reset();
            rightPID.reset();
            leftPID.setSetpoint(left);
            rightPID.setSetpoint(right);
            leftPID.enable();
            rightPID.enable();
        }
    }

    public void spinRateUpdate(double rate){
        spinRateUpdate(rate, rate);
    }

    public void spinRateUpdate(double left, double right){
        if(leftPID.isEnable() || rightPID.isEnable()){
            // PID is enabled, update the values
            leftPID.setSetpoint(left);
            rightPID.setSetpoint(right);            
        }
        else{
            // PID is not enabled, call spinRate
            spinRate(left, right);
        }
    }

    public void spinStop(){
        if(maxPower){
            // Store changes
        }
        else{
            leftPID.disable();
            rightPID.disable();
            spinEffort(0,0);            
        }
    }

}
